#ifndef _UBT_COM_UTIL_H_
#define _UBT_COM_UTIL_H_
#include <string>
#include <vector>
#include <map>

#include "geometry_msgs/msg/pose2_d.hpp"
#include "tf2/utils.h"

namespace ubt_com
{
double getQuaternionYaw(const tf2::Quaternion& quat);

geometry_msgs::msg::Quaternion convertThetaToQuaternion(double theta);

std::string GetVersion();
long GetNowInMsecs();

std::vector<std::string> SpitStr(std::string strOrig, char spitChar);
std::map<std::string, std::string> ParseUrlQuery(std::string urlQuery);

bool isZero(double val);
/*
	弧度(-M_PI,M_PI]转为角度(-180,180]
*/
double rad2Degree(double rad);

/*
	获取（-M_PI,+M_PI]区间内的弧度
*/
double getRegularTheta(double theta);

/*
	从posSrc指向到posDst的theta角(弧度)
*/
double getThetaS2D(geometry_msgs::msg::Pose2D& posSrc, geometry_msgs::msg::Pose2D& posDst);


/*
	 点1在坐标系A中为p1PoseInA,在坐标系B中为p1PoseInB;求点2(在坐标系A中为p2PoseInA)在坐标系B中的坐标
*/
geometry_msgs::msg::Pose2D getP2PoseInB(geometry_msgs::msg::Pose2D p1PoseInA, geometry_msgs::msg::Pose2D p1PoseInB, geometry_msgs::msg::Pose2D p2PoseInA);

/*
	poseLoc,poseBase为两个点在同一坐标系下的坐标,求当以poseBase为坐标系时poseLoc的相对坐标
*/
geometry_msgs::msg::Pose2D getL2BPose(geometry_msgs::msg::Pose2D poseLoc, geometry_msgs::msg::Pose2D poseBase);


/*	使theta从thetaSrc 切换到thetaDst所需的旋转速度,>0 逆时针,0 已到位,不用旋转,<0 顺时针	*/
double getAnglularSpeed(double thetaSrc, double thetaDst, double thetaOffMax);

}
#endif

